#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  HTServo)
#pragma config(Sensor, S2,     IR1200,         sensorHiTechnicIRSeeker1200)
#pragma config(Motor,  motorA,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  motorB,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  motorC,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  mtr_S1_C1_1,     BL,            tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     FL,            tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     BR,            tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C2_2,     FR,            tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C3_1,     Arm,           tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C3_2,     Lift,          tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S1_C4_1,    Wrist,            tServoStandard)
#pragma config(Servo,  srvo_S1_C4_2,    servo2,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"

int white = 1000;
int black = -1000;

/*void Interval25 (int duration)
{
	int time = 0;
	time = duration/25;
	for(int x = 0; x < time; x++)
  {
  	wait1Msec(25);
	}
}
*/
void Forward (int power, int duration)						// Positive power means wheels going forward
{
	motor[FL] = power;
	motor[FR] = power*1.3;
	motor[BL] = power;
	motor[BR] = power*1.3;
	wait1Msec(duration);
}

void Backward (int power, int duration)
{
	motor[FL] = -1*power;
	motor[FR] = -1*power;
	motor[BL] = -1*power;
	motor[BR] = -1*power;
	wait1Msec(duration);
}

void turnL (int power, int duration)
{
	motor[FL] = -1*power;
	motor[FR] = power;
	motor[BL] = -1*power;
	motor[BR] = power;
	wait1Msec(duration);
}

void turnR (int power, int duration)
{
	motor[FL] = power;
	motor[FR] = -1*power;
	motor[BL] = power;
	motor[BR] = -1*power;
	wait1Msec(duration);
}

void initializeRobot ()
{
	motor[FL] = 0;
	motor[BR] = 0;
	motor[FR] = 0;
	motor[BL] = 0;
	wait1Msec(100);
}

/*void ProportionalLineFollowing (void)
{
	while(SensorValue[Ultrasonic] <= 50)
	{
		while(SensorValue[Light] >= 0 && SensorValue[Light] <= 20)
		{
			motor[LeftMotor] = 0;
			motor[RightMotor] = 50;
			Interval25(50);
		}
		while(SensorValue[Light] > 20 && SensorValue[Light] <= 40)
		{
			motor[LeftMotor] = 12;
			motor[RightMotor] = 38;
			Interval25(50);
		}
		while(SensorValue[Light] > 40 && SensorValue[Light] <= 60)
		{
			motor[LeftMotor] = 25;
			motor[RightMotor] = 25;
			Interval25(50);
		}
		while(SensorValue[Light] > 60 && SensorValue[Light] <= 80)
		{
			motor[LeftMotor] = 38;
			motor[RightMotor] = 12;
			Interval25(50);
		}
		while(SensorValue[Light] > 80 && SensorValue[Light] <= 100)
		{
			motor[LeftMotor] = 50;
			motor[RightMotor] = 0;
			Interval25(50);
		}
	}
}


void EncoderMovementForward (int power, int duration) 	//38 inches distance needed to go from start to corner
{
	nMotorEncoder[BR] = 0;
  nMotorEncoderTarget[BR] = duration;
	nxtDisplayCenteredBigTextLine(3, "%d", nMotorEncoder[BR]);			// 962 ticks is one revolution on holonomic chassis
	Forward(power,100);
	while(nMotorRunState[BR] != runStateIdle)
	{
		Forward(power,100);
		nxtDisplayCenteredBigTextLine(3, "%d", nMotorEncoder[BR]);
	}
	initializeRobot();
}

void EncoderMovementDriftR (int power, int duration)		//21 inches from middle peg to right peg
{
	nMotorEncoder[BR] = 0;
  nMotorEncoderTarget[BR] = duration;
	nxtDisplayCenteredBigTextLine(3, "%d", nMotorEncoder[BR]);			// 962 ticks is one revolution on holonomic chassis
	driftR(power,100);
	while(nMotorRunState[BR] != runStateIdle)
	{
		driftR(power,100);
		nxtDisplayCenteredBigTextLine(3, "%d", nMotorEncoder[BR]);
	}
	initializeRobot();
}

void EncoderMovementDriftL (int power, int duration) 	//21 inches from middle peg to left peg
{
	nMotorEncoder[BR] = 0;
  nMotorEncoderTarget[BR] = duration;
	nxtDisplayCenteredBigTextLine(3, "%d", nMotorEncoder[BR]);			// 962 ticks is one revolution on holonomic chassis
	driftL(power,100);
	while(nMotorRunState[BR] != runStateIdle)
	{
		driftL(power,100);
		nxtDisplayCenteredBigTextLine(3, "%d", nMotorEncoder[BR]);
	}
	initializeRobot();
}
*/

int BeaconPost = 0;

void FindBeaconPost(void)
{
	if (SensorValue[IR1200] == 2)
	{
		BeaconPost = 2;
		nxtDisplayCenteredBigTextLine(3, "%d, %d", SensorValue[IR1200],BeaconPost);
	}
	else if (SensorValue[IR1200] > 2)
	{
		BeaconPost = 3;
		nxtDisplayCenteredBigTextLine(3, "%d, %d", SensorValue[IR1200],BeaconPost);
	}
	else if (SensorValue[IR1200] < 2)
	{
		BeaconPost = 1;
		nxtDisplayCenteredBigTextLine(3, "%d, %d", SensorValue[IR1200],BeaconPost);
	}
	else
	{
		BeaconPost = 0;
		nxtDisplayCenteredBigTextLine(3, "%d, %d", SensorValue[IR1200],BeaconPost);
	}
}

void GoToBeacon(void)
{
	if (BeaconPost == 1)
	{
		turnL(50,600);
		initializeRobot();
		Forward(75,750);
		initializeRobot();
		turnR(50,575);
		initializeRobot();
		Forward(75,500);
	}
	else if(BeaconPost == 2)
	{
		Forward(50,600);
		//Here line following will be inserted.
	}
	else if(BeaconPost == 3)
	{
		turnR(50,600);
		initializeRobot();
		Forward(75,650);
		initializeRobot();
		turnL(50,550);
		initializeRobot();
		Forward(75,500);
	}
}

/*bool LinedUp = false;

void CheckForBeacon (void)
{
	while(!LinedUp)
	{
   switch(SensorValue[IR1200])
   {
     case 0:
     case 1:
     driftL(75,100);
     break;

     case 2:
     LinedUp = true;
     break;

     case 3:
     case 4:
     case 5:
     driftR(75,100);
     break;

     default:
   	 break;
   }
  }
}

/*
void LightSensorCalibration(void)
{
	nxtDisplayCenteredTextLine(3, "BLACK");
	wait1Msec(500);
	nxtDisplayCenteredTextLine(3,"RIGHT");
	wait1Msec(500);
	nxtDisplayCenteredTextLine(3,"FIRST");
	wait1Msec(500);
	nxtDisplayCenteredTextLine(3, "WHITE");
	wait1Msec(500);
	nxtDisplayCenteredTextLine(3,"LEFT");
	wait1Msec(500);
	nxtDisplayCenteredTextLine(3,"SECOND");
	wait1Msec(500);

	while(nNxtButtonPressed != 3)
  {
	  if(nNxtButtonPressed == 1)
	  {
	  	black = SensorValue[Light];
			nxtDisplayCenteredTextLine(3,"BLACK %d", black);
	  }
	  else if(nNxtButtonPressed == 2)
	  {
	  	white = SensorValue[Light];
	  	nxtDisplayCenteredTextLine(3, "WHITE %d", white);
	  }
	}
}
*/
task main()
{
	initializeRobot();

	waitForStart();

	servoTarget[Wrist] = 0;
  //LightSensorCalibration();
	//EncoderMovementForward(25, 2909);
	Forward(75,1000);
  initializeRobot();
	FindBeaconPost();
	initializeRobot();
	GoToBeacon();
	initializeRobot();
/*	CheckForBeacon();
	initializeRobot();*/
	motor[Lift] = 100;
	wait1Msec(700);
	initializeRobot();
	motor[Arm] = 40;
	wait1Msec(2000);
	initializeRobot();
	servoTarget[Wrist] = 100;
	wait1Msec(100);
	motor[Arm] = 30;
	wait1Msec(1250);
	initializeRobot();
	servoTarget[Wrist] = 150;
	motor[Arm] = -30;
	wait1Msec(700);
	Backward(30,700);
	initializeRobot();
}
